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I. INTRODUCTION 


This report summarizes the work performed by the Ordnance Department of the Gen- 
eral Electric Company for the George C. Marshall Space Flight Center under contract NAS- 
8-21273. This work was performed over the period from January 8, 1968 to July 8, 1968. 

The basic objective of this study was to investigate digital methods for recovery of 
the initial misalignment of a strapdown inertial navigation system from vibration- and 
sway-corrupted data on the launch pad. The methods investigated under this contract are 
all based on the criterion of a minimum mean square error and differ mainly in the mech- 
anization technique and the amount of a priori knowledge of the statistics which is assumed. 
Methods considered included an optimal filter of the Kalman type, a simplified Kalman 
filter, a Maximum Likelihood approach, and a Least Squares Curve Fit technique with and 
without preconditioning of the data. 

Section II of this report contains a summary of the results of this study, the conclu- 
sions reached regarding recommended filtering techniques, and information to allow trade- 
offs between the two recommended techniques to be performed as more information is ob- 
tained about the noise characteristics and operational restrictions (time, computer capacity 
available, etc.) 

Section III of this report contains a description of the analysis performed for the 
various filtering methods considered. 

Section IV contains a description of the erection and alignment simulation program 
which was provided to the Marshall Space Flight Center to allow further evaluation of 
the two recommended techniques. 

Section V summarizes the results of the work done in determining the computer re- 
quirements for solution of the filter and azimuth alignment equations for the two recom- 
mended filters. 

Appendix A contains a discussion of the form of a strapped down accelerometer out- 
put in a swaying missile and the terms which must be filtered out of this output in order to 
determine the accelerometer orientation. 

Appendix B contains a discussion of the optical alignment equations. 

Appendix C contains a derivation of the closed form solution of the least squares 
curve fit filter equations. 
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A. CONCLUSIONS 

There are several filtering techniques which provide 1 araccuracies better than 
20 seconds of arc with a data-gathering period less than 100 seconds of time. 

The recommended filtering technique is the optimal 4-state filter (simplified Kalman). 
This recommendation is based mainly on a consideration of performance. The performance 
of this filter is equivalent to that of the maximum likelihood filter and has less computational 
complexity. This filter also provides better accuracy in a given time than the least squares 
curve fit approach. If the effects of earth's rate coupling are removed by either updating 
the transformation matrix and releveling, or by using the correction matrix derived in 
5ection lll-F, the performance approaches that of the optimal 12-state filter with a signifi- 
cant decrease in computational complexity required. 

The main disadvantages of the optimal 4-state filter relative to the least squares 
curve fit filter are the launch pad computer capacity and the amount of precomputation 
required. If pad computer capacity becomes a limiting item, the least squares filter 
provides a back-up technique which can be used to alleviate the problem. The linear 
least squares filter using non-integrated data is the recommended approach in this case. 

Gyro drift causes an error which is equal to one half the drift angle accumulated 
over the filtering time in both the 4-state optimal filter and the least squares filter. 

The Marshall Space Flight Center has indicated that gyro drift will be small enough so 
that this error will not be significant. 

Bias errors due to earth's rate crosscoupling exist in both the 4-state optimal and 
least squares filters. These errors can be removed by either updating the transformation 
matrix and releveling or by using the correction matrix derived in Section lll-F. It is 
recommended that the correction matrix approach be taken to eliminate the need for 
additional iterations. 

The maximum likelihood and least squares curve fit filter using integrated data are 
not recommended for further consideration - the former because it is extremely complex 
to mechanize and the later because it is extremely sensitive to sway velocity noise 
correlation time. 


.7" 
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B, RESULTS SUMMARY 


Figures 1 through 10 show the results of the analysis of the Kalman filter. Figure 1 
is the 1 aerection error for the optimal 12-state filter. This filter is described by equa- 
tions 3 - 72 to 3 - 76. The conditions used for this baseline case are given by 

Initial erection uncertainty = 1/2 degree 

Gyro drift uncertainty o'j = . 1 meru 

RMS sway velocity = .5 m/s 

Center frequency of sway velocity power spectrum f = .25 cps 

Correlation time (reciprocal of bandwidth) 
of sway velocity power spectrum r = 200 sec 

Sampling rate 1 sample/sec 

It is seen that the performance of this filter is well within the required accuracy. 

The 1 o'error after 60 seconds of time is less than 6 arc seconds. 

Figure 2 shows the performance of the suboptimal 4-state filter for various initial 
erection angles. This filter is described by equations 3-94 to 3-100, The main simplifi- 
cation employed in reducing the 12-state optimal filter to two identical 4-state filters was 
the neglect of earth's rate crosscoupling terms in the system dynamics. In addition, gyro 
drifts were neglected in the simplified dynamics. The difference in errors for various initial 
angles is attributable to the earth's rate coupling. For initial angles less than 0.5 degrees 
the increase in error over the optimal filter is less than 1 second of arc at 60 seconds time. 
If the initial angle uncertainty is large, the 12 state optimal filter performance can be ap- 
proached by updating the computer coordinate system after about 20 seconds to reduce the 
crosscoupling effects. Another approach to removing the crosscoupling terms is to use the 
correction matrix derived in Section lll-F. 

Figure 3 shows the degradation in performance due to the neglect of gyro drift. It 
is seen that the drift uncertainty must be quite large (20 meru = 0.3 deg/hr) before the 
filter performance is degraded appreciably. 

Figure 4 shows the erection error vs time for various sampling times. A sampling 
time of 1 sec seems to provide a good trade-off between computational requirements and 
erection accuracy. The worst case occurs when the sampling time is 4 sec. This is be- 
cause the center frequency of the sway velocity power spectrum is .25 cps. Since the 
power spectrum is very narrow, the sway velocity is almost sinusoidal with a period of 
4 sec. The sampling is thus at the same part of the sine wave and it becomes quite diffi- 
cult to filter out the noise. 

The correlation time (reciprocal of the bandwidth) of the narrow band noise has a 
significant effect on the estimation accuracy. This is demonstrated in Figure 5. It was 
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Figure 3. 4-STATE FILTER ERECTION ERROR FOR VARIOUS GYRO DRIFTS 
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Figure 4. ERECTION ERROR FOR VARIOUS SAMPLING TIMES 





found that the filter was insensitive to errors in the assumed correlation time. When the 
assumed correlation time was 20 sec. rather than the true correlation time of 200 sec, the 
degradation in filter performance was negligible. The important parameter is the actual 
noise correlation time and not the correlation time assumed for the filter calculations. 

Since the frequency of the missile swcy may not be known exactly, it is of interest 
to determine the sensitivity of the filter to that parameter. This is shown in Figure 6. 

In this case, the actual frequency is .25 cps while the number used in the filter calcula- 
tions is .2 cps. It is seen that the accuracy of the estimate is somewhat poorer, but 
still acceptable. The performance for an assumed frequency of .3 cps is almost identical 
to the curve shown in Figure 6 and, therefore, has not been presented. 

An alternative method of erecting the analytic coordinate system is to use a least 
squares filter. The computational requirements of this method are much less than those 
of the optimal filtering technique. The estimate, of course, takes longer to converge 
to within acceptable limits. Figure 7 compares the performance of the least squares and the 
optimal 4-state filters of the case when t = 200 sec. The optimal filter is significantly 
better than the least squares filter for this case. Figures 8 and 9 show the same comparison 
for T = 20 sec and 5 sec respectively. It is seen that as t decreases, the least squares 
filter performance approaches that of the optimal filter. This is to be expected since, 
for white noise (t = 0), the least squares filter is optimal. 

It is possible to further reduce the number of states in the filter from 4 to 3isy 
sampling at exactly 1/2 the period of the sway velocity. This happens because some of 
the terms in the filtering equations are multiplied by sin cot which is always zero at 
co= tt/ 2. Figure 10 shows the 3-state filter performance. The sensitivity to using the 
wrong f in the filter equations Is also shown. While the filter performance is accept- 
able, it is felt that constraining the sampling time to 1/2 the period of the sway velocity 
is too restrictive. A more flexible filter seems desirable. 

The preceding curves were calculated by determining the covariance matrices for 
the estimation errors using the results of Section lll-C-2. Figure 11 shows the results 
of a sample run from the Erection and Alignment Simulation Program described in 
Section IV. In Figure 1 1 the sway velocity noise is assumed to be in a narrow band 
around a center frequency. Figure 12 shows the results of a run in which the sway velo- 
city noise is assumed to consist of 3 narrow band noises at different center frequencies. 

The center frequencies for the bands are 1 .82 rad/sec, 2.14 rad/sec and 2.32 rad/sec 
with a correlation time of 200 seconds for each band. The total rms sway velocity is 
0.5 m/sec. This run was made to provide assurance that the filter would operate properly 
with a more realistic noise input. 
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Figure 6. SENSITIVITY OF 4-STATE FILTER TO ERRORS IN ASSUMED 
CENTER FREQUENCY OF SWAY VELOCITY 
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Figure 9. A COMPARISON OF THE 4-STATE AND LEAST SQUARES FILTERS 
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Figure 13 shows the results of the analysis of a least squares curve fit filter. This 
analysis was performed by a Monte Carlo computer simulation as described in Section I II— D. 
The curve labelled non integrated data is derived by fitting the velocity data from the ac- 
celerometers to a constant plus a linear time term. The curve labelled integrated data is 
derived by fitting the integral of the velocity data to a quadratic function. The asymptotes 
on Figure 13 are the errors when the noise is considered to be a sine wave of a single 
frequency (r = 00 ). 

The results of the maximum likelihood filter analysis are contained in Section lll-E. 
These results in general confirm the results of the Kalman filter analysis and are not re- 
peated here. 


Computer storage requirements are 636 locations if the optimal 4-state filter is used 
and 342 locations of the least squares filter is used. These numbers include provision for 
the azimuth and correction matrix calculations. Execution times are tabulated in Section V. 
Fixed point calculations with the 23 bit word length of the RCA-110A computer are 
acceptable. 

Certain "rules of thumb" which have been developed, to allow extrapolation of the 
results given in this report to different noise characteristics and filtering times are given 
below. 


a) 

b) 


c) 

d) 


The rms filtering error is proportional to the rms value of the sway velocity noise. 
If earth's rate crosscouplings are removed by updating or by using the correction 
matrix, the filtering error is not a strong function of the initial angle error 
(See table 2, Section lll-D). 

The filtering error is approximately inversely proportional to the noise center 
frequency (See Section lll-E and Equation 3-135, Section lll-D). 

For short correlation times, the filtering error for both the optimal 4-state 
and least squares filter is approximately inversely proportional to the three 

halves power of the filtering time (Error = ^ ) . This is shown by plotting 

T372 

the results of Figure 8 on log-log paper in Figure 14, For longer correlation 
times, the least squares filter has a filtering error which is more nearly inver- 
sely proportional to the square of the filtering time while the 4-state optimal 
remains inversely proportional to the three halves power of the filtering time. 

This is shown by plotting the results of Figure 7 on log-log paper in Figure 15. 

It must be remembered, however, that the curves for the two filters can never 
intersect so that the least squares filter error must approach being inversely 
proportional to the three halves power of filtering time as the time increases. 
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Figure 14. DEPENDENCE OF THE FILTERING ERROR ON FILTERING TIME (r - 20 SECONDS) 
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DEPENDENCE OF FILTERING ERROR ON FILTERING TIME (r = 200 SECONDS) 
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e) The filtering error for the optimal 4-state filter is approximately inversely 
proportional to the square root of the sway velocity correlation time (Error 

= The | eas f S q Uares filter error only approaches being inversely 

T / 2 

proportional to the square root of the correlation time at low correlation times, 
however. For higher correlation time the improvement in performance with 
increasing correlation time is much less than that of the optimal 4-state filter. 
This is shown by plotting the results of Figures 7, 8, and 9 at 60 seconds time 
on log-log paper in Figure 16. This rule obviously cannot be used as r 
approaches z' o but it does hold at least down to t = 1 second. 
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DEPENDENCE OF FILTERING ERROR ON NOISE CORRELATION TIME 
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Ill, FILTER ANALYSIS 


A. SYSTEM DYNAMICS 

Introduction 

The equations describing the strapdown erection process are derived in this section. 
These include the differential equations governing (1) the euler angles describing the mis- 
alignment between the earth-fixed launch-site coordinate system (CS) and the strapdown 
inertial reference CS, (2) the transformed accelerometer outputs, and (3) the missile sway 
velocity. The differential equations are then converted to a set of first order difference 
equations since this is the form appropriate for the application of the discrete-time Kalman 
filter equations. Certain assumptions are made In this analysis. These are: 

1 . The initial miserection angles are small enough so that the small angle approxi- 
mations (sin 0= 9, cos 0= 1) are valid. 

2. The gyro drift rates are constant. 

3. The sway velocity is adequately represented by a narrow-band noise process. 

The Differential Equations Governing the Euler Angles Describing the 
Misalignment Between the Earth-Fixed Launch Site Coordinate System 
and the Strapdown Inertial Reference Coordinate System 

The differential equation describing the direction cosine matrix between two rotating 
coordinate systems is given by: 

JCi = Jci Oj - «j JCi 3-1 


Where: jq is the direction cosine matrix which transforms a vector from CS(i) to CS(j) 
fl; is the skew symmetric matrix of the angular rates of C$(i). 
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The three coordinate systems of interest in this analysis are the earth-fixed launch-site CS, 
the strapdown inertial reference CS, and the strapdown inertial instrument CS. These are 
designated as CS(0), CS(1), and CS(2) respectively. 


The coordinate system CS(1) is the CS to which the outputs of the body-fixed ac- 
celerometers are transformed. This CS is defined by the equation which the CTMC solves,, 
namely: 

'C 2 = ’C 2 ij> - £TC 2 (3-2) 

Where: i/i is the matrix of angular rates measured by the gyros 
is the matrix of earth’s rates in CS (0). 

The equation for 3 C x = 3 C 1 fl - lp 2 C 1 (3-3) 

The gyro measured rates are in error by the gyro drift. Thus: 

1 = (B> + D 3 (3-4) 

Where: (0) is the matrix of true angular rates 

D 3 is the matrix of gyro drift rates 

Substituting equation 3-4 into 3-3 gives: 

2 q = 2 c x o = (0) 3 q - d 8 3 c a (3-5) - 

The direction cosine matrix 0 C s is given by: (3-6) 

°c 2 = °c s ( 0 ) - n °C S (3-6) 

One can write the direction cosine matrix °C 1 as: 

0 Cj + U C 2 3 C 1 (3-7) 

Differentiating equation 3-7 yields: 

°C X = °C S 3 C 1 + °C 2 2 C 1 (3-8) 

Upon substituting equations 3-4, 3-5, and 3-7 into 3-8 one obtains: 

°C 1 O-O 0 C 1 -°C 2 D 3 3 C 1 (3-9) 
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The last term in equation 3-9 is rewritten as: 


U C S D 2 y C 1 = °C 1 l c a D 3 x c 3 =°c 1 Dj 

(3-10) 

Thus equation 3-9 becomes 


°C 1 =°C 1 0-n°C 1 -°C 1 

(3-1 1) 


The transformation between CS(0) and CS(1) is defined by the three euler angles 
a, $, and y , These are shown below in Figure 18. 



When the small angle approximations are employed the matrix °C a can be written as 


°C! = 


1 

y 

-J3 


-y 

l 

a 


0 

-a. 

1 


Substituting equation 3-12 into equation 3-11 yields: 


0 

y 

-0 


-y 0 

0 -a 
& 0 


(3-12) 
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From equation 3-13 one easily obtains the following three simultaneous differential 
equations: 


a 

4 


_y_ 



0 

k'z 

-u> 


— 0) 2 

0 

y t.o * 


co 

0 


y 

Ct>X 


a 

0 

y 


dx 

d v 


In matrix-vector notation, this is written as: 
9 = -a 9 -d 


(3-U) 


(3-15) 


Where: 



a 


~ d x 

9 = 

y 

d = 

IaI 


The Differential Equations Governing the Transformed 
Accelerometer Outputs 


The CTMC output gives the integral of acceleration along the coordinates of 
CS(1). In particular, the outputs along the y and z axes are given by: 

Pi W = / fg/3( T ) + v 1 (t)] dT 

to 

Pa (0 = / Q-g y (r) + v 2 (rQ dr 

to 

Where: 

p t (t) is the output along z x at time t 
p s (t) is the output along y x at time t 
'J’i (t) is the sway acceleration along Zj 
$ s (t) is the sway acceleration along y x 
g is the value of gravity (m/sec 3 ) 


(3-16) 

(3-17) 


Differentiating equations 3-16 and 3-17 yields the desired differential equations: 


Pi = g ^ + vj 

(3-18) 

p 8 = -g y + v s 

(3-19) 

p = B 9 + v 

(3-20) 
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Where: 


P = 





The Differential Equations Governing the 
Missile Sway Velocity 


It is assumed that the power spectrum of the sway velocities along the y, and z, 
axes is adequately described by: 

V? , 1A 


Sy, (w) “ Sy (u>) — 


Cf> 


[ 


(c0“C0o) 3+ (1/r) 3 


(u>+u> 0 ) ; 


+ (l/r) : 


3 

» — i 


(3-21) 


Where a v is the rms sway velocity. 

This power spectrum is shown in figure 3-3. 

It can be shown also that if nj (t) and n 2 (t) are independent random variables with 
power spectra 



2 av 3 /r 
a) 3 + (1/rf 


(3-23) 


Then the random variable 

v i (0 = n i (0 cos a»o t + n s (t) sin u>o t (3-24) 

will have the power spectrum shown in Figure 19. 

Noise with the power spectrum shown in equation 3-23 has the corresponding 
autocorrelation function: 

= Rn s (t> = ffv e " ,H/t (3-25) 

Noise with this autocorrelation function can be considered as being the 
solution to the stochastic differential equation: 

(t) = (-I/t)^ (t) + w T (t) (3-26) 

h 2 (t) =(-l/r)n s (t) + w 2 (t) (3-27) 
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Figure 19. POWER SPECTRUM OF SWAY VELOCITY 


(3-28) 


Where w x (t) and w 3 (t) are white noise random variables with autocorrelation 

E Q w i (h) w i O 2 Q ° EQw 3 (t x ) w 3 (t 3 fj = 6 (t x -t 3 ) 

6 (t 1 - t 3 ) is the dirac delta function. 

v x (t) is the sway velocity along the z axis. For the sway velocity along the y 
axis, v s (t), we have similar equations: 

v 3 (0 = n 3 (t) cos a> 0 1 + n 4 (t) sin u> 0 t 

n 3 (f) " “1/r n 3 (t) + w 3 (t) 

n 4 (0 = “1 /t n 4 (t) + w 4 (t) 

Where: 

E [w 3 (fi) w 3 (t s j] = E [w 4 (t x ) w 4 (t a )] = ~L. 6 (t x -t 3 ) 

In vector notation, then, we have 
v(t) = C(t) n(t) 
n(t) = An(t) + w (t) 


Where: 


v(t) = 


v a (t) 
v 2 (0 


C(t) 


■D 


cos a>o t sin a>o t 

0 0 


0 0 

cos u>ot sin a»of 


a 


A = (-1/r) I 


Conversion of the Differential Equations to 
Difference Equations 

The differential equation 

x(t) = F x (t) + w(t) 

has the solution 

fi 

x(fi) = 0 (fi/ tj_ x ) x (t{- x )+ J 0(t { ,A) w(X) d\ 

f i-i 


(3-29) 

(3-30) 

(3-31) 

(3-32) 

(3-33) 

(3-34) 


(3-35) 

(3-36) 
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(3-37) 


Where 0 (tj, t|_ t ) is the matrix exponential 
0 (V tj-i) “ exp Q(tj -tj_x) f) 

Equation 3-36 can be rewritten 

X (tj) - 0(tj, tj.j) x (tj^) + u (tj) 

Where: 

u(t|) = / 0 (tj/ X ) w ( X ) dx 

*l-i 

Equation 3-38 is thus the difference equation which is equivalent the 
differential equation 3-35. 


Using this technique, the solution to equation 3-15 is given by: 
© (tj) = 0n (tj/ tj-i) 6 (tj-i ) 


$ 0n(t|/X)dX 
f i-i 


Where 


0ii (tj/ ti-i ) = exp£- (tj-tj_ t ) n] 
Equation 3-40 can be rewritten 


e(tj) = 011 (tj, tj_ a ) e (t| wl ) + 0* (tj, t( n ) d(tj) 

Where d(t;) is constant vector and thus satisfies the difference equation 

d(t|) = d(t|-j) 


The matrix 0 13 (t; , tj^ ) is given by: 

0i s (tj/ tj„j)= - / 0ii (tj, X) d X 

tj-i 

A solution for 0 lt (tj, tj_ t ) is obtained by using the Cayley-Hami Iton 
method for calculating matrix exponentials. The solution is given by 

0„<t|, *!-,) - i - [i-Jtouj] n 

*[- 


t0(tj ~ tj-i ) | q£ 

<o 3 j 


(3-38) 


(3-39) 


(3-40) 


(3-41) 


(3-42) 


(3-43) 

(3-44) 


(3-45) 
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Where o> is the earth's rotation rate. 


The matrix 0 13 (tj, t| ) is obtained by integrating equation 3-45. The solution is 


'is 


(tj, tj- a ) « -I (tj 


-tj-,) + ■ 


1 - cos U>(tj - tj- 1 ) 




O 


(3-46) 


- r (ti " t|-i) _ sin a> (tj - tj wl )~\ _ 2 

Li?— — 5 s — - J 


Difference equations describing the noise will be derived next. The solution to 
equation 3-34 is given by: 


n(tj) = 044 (ti/ tj- a ) n (tj.,) + / &4 (ti, A ) w ( X) d X 


(3-47) 


ti-i 


Where: 


"44 


(ti, t;„j ) = exp jjftj - tj_ a ) a] = e " ti_1 


Equation 3-47 can be rewritten as 
"(tj) = 044 (t }/ tj.j ) n(tj. a ) + u(tj) 

Where : 

ti 

u(tj) = / 044 (tj, A ) w ( X) d X 

tj»i 

u(tj) is a vector random variable. Its covariance matrix is given by 
Q(tj) = E [u (tj) u *(t|)TJ 


(3-48) 


(3-49) 


(3-50) 


tj tj -(t 5 _ x )/ f 

= / / e 


tf-i t;„. 


r - -(t; - A„)A 

E Qw(Xj) w'(X 3 )je ' d X f d A s (3-51) 


Since 


E [*(*,) w'(A 2 )] = a (A, - A a ) 

equation 3-51 reduces to a single integral 

,1 e' 2(, i' X . )/t dA, 


(3-52) 


Q(tj) = 


ti-i 
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The solution to this integral is given by 

Oft,) - a/ 0 ' 

Finally, a recursive expression for the accelerometer outputs must be obtained. 
Equation 3-20 can be integrated to give: 

p(ti) = B /' 0 (A) dA + v(tj) - v(t Q ) ' 

*o 

This equation can be rewritten as: 

p(tj) = B 0(A) dA + p(ti„x) + v(tj) - v(t|_ a ) 


Substituting equation 3-42 into equation 3-56 gives 


p(tj) - B / 0u(ti, A) dA 9 
t:_. 


+ B / 0-ls (t; , A ) dA \ 

V 

+ p(t;_ 1 ) + v(tj) - v^.J 

J 

This equation can be rewritten as 

p(t ; ) = 03i (fy f i-i) 6 ( f i-i) + 0 3s ^i r *‘i-^ d ^i-^ 

+ p(t;_!) + v(tj) 


Where: 


031 (tjr tj.0 = B / 011 (V A) dA 

M-i 

= “B 0i3 (t}, ti-i) 


(3-59) 
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.4, ((„»,.,) = 6 / 4 a < f i. x > dA 


ti-i 


_ v. . 


Rn-ti-i ) 2 _ 

1 - cos a>(t; - tj_i) 

1 n 3 ^ 

L 2 co 2 

to 4 



Equations 3-33 and 3-49 are substituted into equation 3-58 to give 
p(tj) = 031 (n, tj.J 9 (tj_ T ) + 033 (tf, ti-i) d (ti-i) 

+ p(t|-i) + 034 (V ^i-i) n ^i-i) 

+ C(tj) u(tj) 


Where: 


03 4 (t j / tj _^ ) - C (t;) 0 44 (tj, t|_ a ) - c (tj _a) 

Combining equations 3-42, 3-43/ 3-49 and 3-61 gives 

0ii O'!# I’i-i) 0i3 ^1/ l?-i) 


9(tj) 

d(t,) 

P(t|) 

n(t|) 


1 v I / I • 

0 I 


+ 


“3 1 V|/ 

0 

" 0 " 
0 

C(t|) 


0 


1 

o 

o 


e(ti-i)" 

o 

o 


d(ti-i) 

I 03 4 0*1/ 


p(ii-i) 

0 044 0 "J/ ^|-l) 


n(ti-x) 



u (tj) 


This equation is written: 

x(tj) = 0(t|, t|_j) xitj.i) + G(t;) u(t ; ) 


Where: 


(3-60) 


(3-61) 


(3-62) 


(3-63) 


(3-64) 


x(t|) = 


e(tj) 

d(tj) 

p(*i) 

n(t;) 


00i t ^i-i) 


0i i O'} / tj-i) 0i 8 (»f#tl-i) 0 0 

0 10 0 

j4i(^/ f i-i) 0Bs(V f i-^ 1 034( t i/ t i-i) 

0 0 0 0 44 (t, ^.j) 

(3-65) 
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The measurement consists of the two transormed accelerometer outputs ♦ There is 
noise on the measurement due to quantization of the accelerometer outputs. The measure- 
ment equation is given by: 


y(tj) = H(tj) x (tj) + r(tj) 


Where: 


H(t:) = 


To o o 

1° 0 0 


0001 00000 
0000 I 0000 


r(tj) is the noise due to quantization. Its covariance matrix is given by 


R(tj) = E fr(t|) r'(t;f) = 

12 

Where A is the quantization level. 


~1 o" 
0 1 

M. 


When a theodolite measurement is taken and processed, the resulting quantity 
is the azimuth misalignment plus an error which is equal to the levelling error about the 
projection of the LOS in the horizontal plane multiplied by the tangent of the elevation 
angle. Thus when a theodolite measurement is taken, the H(tj) matrix is given 

H(t;) = Ql tan y 0 sin £ tan 7 0 cos £ 0 0 0 0 0 0 0 0 o] (3-6 

Where £ is the angle between the projection of the LOS in the horizontal plane 


In this section, a description of the system dynamics has been derived. The 
description is in the form of simultaneous first order difference equations. These are 
presented in matrix form by equation 3-64. The measurements on the system are 
described by equation 3-66. In addition expressions for the various covariance matrices 
needed for the filter equations have been obtained. 

B, OPTIMAL FILTER EQUATIONS 

In part A of section III, the system equations were derived. They are repeated 
here for convenience. 


x(t.) = 0(tj, tj.i) x (t- Ul ) + G(tj) u(t|) 
y(tj) = H (tj) x (h) + r (tj) 
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The well known Kalman Filter equations can be applied to systems with this 
description. These equations give an estimate of the state vector x(t[), based on the 
measurements { y(tj) — y(tj ) } , which is optimal in the mean square sense. The filter 


is described by the following difference equations: 

x (tj, tj_ x ) = 0(t ; , tj_,) x(t ; _,) (3-72) 

x (tj) = x (tj, t._ x ) + K(tj) [y(tj) - H (tj) x (tj, t;_ x )3 (3-73) 

The optimal filter gains K(tj) are calculated from the following set of 
difference equations: 

P(V *i-i) = f i-i) 0 (*\' »i-i) + G(tj) Q(t;) G'(tj) (3-74) 

K(tj) = P(tj, tj_,) H 1 (tj) (tj) P(tj) H'(tj) + R(tj)] _1 (3-75) 

P(tj) =Q- K(tj) H(t.f) p(tj, t..,) (3-76) 


Where: 

x(tj) is the optimal estimate of x(tj) 

given the measurements { y(t,), y(t 2 ), — y(tj) } 

x(t;, tj_,) is the optima! estimate of x(tj) 

given the measurements { y(t,), y(t 8 ), — y(t; ) } 

P(tj) is the covariance matrix of the error in the estimate x(t-) 

P(t;, tj_x) is the covariance matrix of the error in the estimate x(tj, tj.,) 

The above equations describe the optimal filter. A direct application of these 
equations could be done. It is desirable, however, to simplify and decouple the 
equations to reduce the computational requirements. The performance of the optimal 
filter is described in section II . It is also demonstrated that the degredation caused 
by certain simplifications is negligible. 

These simplifications will be investigated in the following section. 

C, SUBOPTIMAL FILTER EQUATIONS 
Introduction 

The filter equations presented in the previous section are rather complex. They 
involve the solution of 12 simultaneous difference equations and hence multiplication of 
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12 x 12 matrices. It would be desirable, then, to investigate the possibility of simpli- 
fying these equations. In order to determine if the simplifications are acceptable, one 
must calculate the degradation in filter performance caused by them. In addition, the 
sensitivity of the filter to errors in the noise model should be investigated. The equa- 
tions required to evaluate the suboptimal filter performance are derived in this section. 
The approximations to the system dynamics which are used in the suboptimal filter are 
also presented. A computer program has been written which solves these equations. 
Results from this program are given in section II. 

Evaluation of Suboptimal Filter Performance 


The equations describing the system dynamics (equations 3-64 and 3-65) are 
repeated here for convenience. 


x(t|) = 0(t ; , t|_ x ) x (tj _ a ) + G(tj) u (tj) 
y(tj) = H (t|) x (tj) + r (tj) 

A fairly general model for the suboptimal filter equations is given by: 
x s (V *j-i) = *i-i^ x s fy-i) 

x s (tj) = x s (tj, tj_x) + K s (tj) £y(tj) - H (t;) x s (tj, t.^f) 

Where K s (tj) is obtained from: 

P $ (tj, f i-i) = #s ^i/ + G s( f i) Q s(tj) G s( t |) 

K s (f;) - P S (V f;_ a ) H 1 (tj) [H (t.) P(t.) H'(tj) + R(tj)J " l 
p s (ti) = [I - K(tj) H (tj)] P(t { , t;.x) 

The following covariance matrices are defined: 


(3-77) 


(3-79) 

(3-80) 

(3-81) 

(3-82) 

(3-83) 


Pi (tj) = E Cx(tj) x'(tj)I) 

P 8 (tj, t;_ x ) = E [* s (tj, tj_ x )3 

p 8 (tj) = E £$ s (tj) X- (t,Q 

Ps (tj, tj_ x ) = E (x s (tj, tj.x) X s '(tj, tj_ x )3 

P 3 (tj) = E £^(tj) fc s ’ (t,Q 

The covariance matrix of estimate errors is given by 

T(t.) = E{ Qx(t,) - ^ (t.)] Qtf,) - !*,(*,)] ’ } (3-84) 
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Equation 3-84 can be written in terms of the covariance matrices defined above. 


P( tj) = Px (U - P s (tj) - P' 8 (t.) + P 3 (t.) 


(3-85) 


By multiplying equation 3-77 by its transpose and taking expected values of 
both sides one immediately obtains: 

Px (tj) t. J 0'(t;, t;_x) + G(t|) Q(tj) G'(tj) 


(3-86) 


Multiplying equation 3-79 by the transpose of equation 3-77 and taking 
expected values yields: 


P 8 (ti, fi_x) = ^ s (fi/ fj_i) p. (tj-x)0'(V f j-i) (3-87) 

The equation for P 3 (t|) is obtained by multiplying equation 3-80 by 
x'(tj) . It is given by: 

p s (tj) = [i - K s H(t.rj P s (t ;/ tj.J + K s H(t.) p,(t.) • (3-88) 

Multiplying equation 3-79 by its transpose and taking expected values gives: 


P 3 (f|r ^i-i) ^s^i' P® ^i -l ^ f 


(3-89) 


The equation for P 3 (tj) is obtained by multiplying equation 3-80 by its 
transpose and taking expected values. After some algeb aic manipulation one obtains: 

P 3 (tj) = P 3 (t|, ti-x) + K s (tj) H(t.) [p a - (tj) - P 3 (tj, t;_x)] 

+ [> 8 (tj) - P 3 (tj, t,.,)] H'(tj) Kg' (tj) 

+ K s (tj) H (tj) QP 3 (tj, tj_x) - P x (t,)3 H’(tj) K s ( (tj) 

+ K s (tj) R (tj) K s ' (tj) (3-90) 


This completes the derivation of the equations necessary to compute the 
covariance matrix of estimation errors. The computer program solves equations 
3-85 through 3-90 in addition to computing the suboptimal filter gains. 


Approximations to System Dynamics 

If earth's rotation rate is neglected (a) = 0), the system dynamics are considerably 
simplified. 
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assse 




The state transition matrix becomes 


i 


0(*t, = 


I 

0 

B(tj “ tj-! 


-(t, - fr|- a )I 


0 o 

0 0 

1 03 4 (fj / ^ i — i) 
0 


(3-91) 


The result of this simplification is that the equations for a , 0, and y become 
coupled* If the drifts are neglected, a further simplification results. The equations 
describing $ are given by 


Mi 

r mh 

1 

Pa (t|) 


n a ( f i) 


_n 2 (tj)_ 

§ 



I 


1 0 0 0 " 

1 #23 02 4 

0 0 0 0 

0 0 0 s3 0 44 




~o o 

pV> , 

+ 

O’ 1 G 2 3 

n a h-i) 


1 0 

__ n s(t;_i)_ 


0 1 



Where: 

| 

033 = 0 44 = eX P " " Vl 

0 33 ~ 0 33 COS 0>O tj - COS 60 O fi-1 

f 034 = 033 sin a>o tj - sin a >0 t;_ a 

G Sl = cos ui tj 

G . • o , 

22 - sin 40 o tj 

!1 




% 


The equations describing y (tj) are of almost the identical form with a sign 
difference. This sign difference can be eliminated by defining p 3 (t|) as minus the 
accelerometer output along the y s axis. 


(3-92) 


: < v ; ' 




I 

I 


The equation for a is simply 

a(tj) - a(tj_ t ) (3-93) 

The filter corresponding to these uncoupled dynamics is also uncoupled. 

This greatly reduces the computational requirements. The filter equations 
corresponding to these simplifications are given by: 
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><i (*|> M-i) = *i“i) 

x 2 (tj, t;_ 1 ) = 0*(tj, 1-j_! ) % (^i-i ) 

^(tj) = x^tj, t j — i ) + K*(tj) Jj^O-j) " H*(t|) Xj(tj, f;_i)3 
x 2 O4) = x 2 (tj/ t|_i) + K*(tj) [- P2 (t;) - H* (t j) x 2 (t;, t j 


P*(tj, tj^) =^*(fr 8 , tj_ 5 ) P*(f-|_j) M-l) + G *( f i) Q*(»|) G *'(n) 
K*(tj) = P*(tj f f,^) H*'(tj) Qh*'(»|) P*(t l# frj-x) 

p*(tj) - Q - K*(tj) H*(t|0 P*(t,, tj-j) 


Where: 

0*0], t|-a) = 


0 


g(*i - *j-i) 
o 

o 


000 

1 0SB 034 

0 033 0 

0 0 0 UJ 


(3-94) 

(3-95) 

(3-96) 

(3-97) 

(3-98) 

(3-99) 

( 3 - 100 ) 



H*(tj) = Qo 1 0 o] 


R*(tj) = A 3 /1 2 


BIAS ERRORS IN THE SUBOPTIMAL FILTER 


The three Euler angles describing the misalignment of the inertial reference CS vary 
with time. This is caused by the earth's rate crosscoupling effect. This effect is neglected 
when the simplifying approximations are made and thereby causes a bias to occur in the 
estimates made by the si. olified filter. 

The quantities one wishes to estimate are the Euler angles at the final time, tf, 
since this is when the CTMC will be "torqued" tt> drive these angles to zero. The relation 
between the Euler angles at time t and those at time tf is given by: 

0(0 = 0U (t, t f ) e (t f ) (3-101) 

The accelerometer measurements p(tj) are thus related to 9(tf) by: 


f i 

pfy) = f 0)) (A, t f ) d A 0(t f ) 

*o 

+ V(t.) - V(t c ) 

It is assumed in the simplified dynamics that: 

^11 (h# f 2) = 1 

The assumed measurement is that given by: 

*i 

P*(ti) = B / I d X ©(tf) + V(tj) - V(t Q ) 

Thus, the actual measurement differs from the assumed measurement by: 


(3-102) 


(3-103) 


(3-104) 


b(tj) = p(tj) - P*(t}) = B / (A, If) - I J dx e (tf) (3-105) 

f o 

This measurement bias causes a bias in the estimates of (tf) and y (tf). 

A correction for this bias can be made. At time tf, there are three quantities 
available from which we can estimate &, £ and y . The first quantity is the theodelite 
measurement. This is given by 

m i = a (tf) + C] /9 (tf) + C 2 y (tf) (3-106) 
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The other two quantities are the biased estimates of £ (tf) and y(tf). They are 
given by; 

m 2 = £(tj) 
m 3 = yfrj) 

Using these three "measurements" we can obtain a minimum variance estimate 
of 0 (tf) which will be unbiased. This estimate is related to the measurements by 


9 (t f ) = A 


Where; 


To calculate A, one employs the principal of orthogonality which states that 
if 0 (tf) is to be a minimum variance estimate of 0 (tf), then 

e C0f> - %>3 m ' } = ° 

Substituting 3-109 into 3-110 and solving for A gives 


0m 'mm 


Where 


Pem - E Of)™'] 

P mm = E L mm ' 3 

The covariance matrix of the crosscoupling estimate is given by 

p eS = Pge - AP' 6m ( 

The correction matrix given by equation 3-111 provides the optimum correc- 
tion in a least squares sense. However, a simpler correction matrix that produces ade* 
qUate accuracy is derived in Section lll-F. Since this correction matrix is relatively 
easy to calculate, it has been used for both types of filters. 
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D, LEAST SQUARES CURVE FIT FILTER 
Introduction 


This section presents the results of an investigation of the use of a least squares 
curve fit filter for levelling a strap-down system in a swaying missile. The objective of 
this investigation was to determine the simplest filter which would produce acceptable 
accuracy with a final data gathering period of less than 100 seconds. The simplest filter, 
of course, would be one in which the accelerometer pulses were merely counted and 
divided by time. However, a simple calculation shows that the error using this technique 
at the end of 100 seconds of time is much too large. The next simplest candidate filter 
is one in which the accelerometer output is assumed to be a power series in time. The 
accelerometer output data can then be fit to this power series in a manner which mini- 
mizes the square of the error. No a-priori knowledge of the form of any noise which 
corrupts the accelerometer output is assumed in deriving the estimated coefficients of the 
power series. However, it can be shown that the coefficients are the same as would be 
derived using a maximum likelihood approach with a white noise input. 

Least Squares Curve Fit Equations 

The sum of the accelerometer outputs at time t is given by 

V = a Q + a^t + a<j t^ + a n t n - mA 


where 


m = [ 1, t, t 2 , .... t n ] 

A . - 0 

a l 
a 2 
a 3 


Now if K data points are taken at times, tj, the error in the ith data point is 
e< = Vj - m t ft 
where A is the estimate of A 

Then the sum of the square of the errors, which is the function to be minimized is 

K A 

e^ = Z (V; - m;A)^ 

>1 
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r 

l 

ii. 


Differentiating with respect to A and setting equal to zero yields 
- -2 E rri| T (Vf - trijA) = 0 


E mj T (Vj - mjA) = 0 


Equation 3-115 represents a set of n + 1 equations of the form 


E (V. - mjA) = 0 


E t( (V[ - mjA) - 0 
i=l 


E tj n (V. - m ; A) = 0 
i=1 

The solution of equation 3-115 yields 


K A K 

T A T 

E m[ 1 m[A = S m- 1 vs 

i=l i=l 


V T 

A = E m. 1 m. 
1=1 


•1 K 

S mj T V. 
i=l 



Et, Et s 2 .*.Eh n ' 


Et j Et } 2 . • • 


StiVj 

Eti 2 V| 

Eti n V; 


Equation 3-116 is the basic least squares curve fit equation used in this technique. 




Physical Model 

The acceleration sensed by an accelerometer at a small angle 9 from the horizontal 
and with zero sway velocity is 

a = 6g 


Consider the coordinate system of Figure 20. In genera! 

9 = 9 + / 0 dt S 

o 

9 = 9 d + eW e2 = 6W ei 

€ = € 0 + / edt 

c = ed + 6W el - 0W e2 

6 = 6 0 + / 6 dt 

fi = fid + 0 W ei " €We 2 


(3-117) 


Here € , 6, 9 are the angular misalignments, the sub 0 quantifies are initial 
misalignments, and the sub d quantities are due to gyro drift. 

For simplicity in determining the form of the accelerometer output, assume that 
the 3 axis i$ North, then W 6 2 “ 0 and 

9 = dj -6 We] 

Ignoring second order effects 

« = #. + W, » 

and 


9 = 8d ' 6o w ei - 9oW e 2 , t 

8 = 9 0 + (0 d - 5 0 W e , ) t - 1/2 6 0 W2 e] t 2 

The sum of the pulses is the integral of a and is given by 

V = g C 8 0 1 + 1/2 (6 d - 6 0 w e| )t 2 - 1/6 8 0 W e 2 l t 2 ] 


(3-118) 



Now if sway velocity is considered including the fact that the velocity output of 
the accelerometer has an average value, 3-1 18 can be written 

V = V Q + g [e o t + 1/2 (9 d •• 5W el )t 2 - 1/6 6/^2 t 3] (3-119) 

. If V is fit to a curve of the form V = + a]t + c^t^ + . . . the initial misalign- 

ment angle, 0 O , and rate of change angle, g f can be determined as 

Q 0 = (3-120) 

g 

9 = ^2 02 (3-121) 

g 

Effect of Sinusoidal Noise at a Single Frequency 

If the noise is assumed to be a sinusoid at a single frequency and data is taken at 
fixed time increments, a simple paper analysis can be performed to determine the errors 
as a function of noise magnitude, noise frequency, time of filtering and order of fit. 

Assuming the data is taken at times 6t, 26 1, . the various terms in equation 

3-116 become 


Li = K 

L t j = 6t [1+2 + 3+ ...] = 
St 2 j = fit 2 [1 + 4 + 9 + ...] 


r . , K (K+l) 

6t 2 

= 6 t 2 K(K+1) ( 2I < + ]} 


etc. 


Now if K »1 (many data points are taken) at time T = Kfit 


Si = 


1 


fit 


Stj = fit K 2 /2 = . ~^ 2 
1 fit 


St 


2. = 1 T 3 /3 


fit 


etc. 
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A 


I 


T 

/ dt 

0 


f t dt 

0 


vV _ i r v.dt 
sV i ■ W ! ' 


EtV. = f tV.dt 

, 1 ot I 


And 3-116 becomes 


T 2 /2 ... 


T 2 /2 T 3 /3 ... 


J V.dt 
0 


/ t V; dt 

0 

/ t 2 V; dt 


(3-122) 


Now suppose the data is given by EVj = a Q + a jt + a 2 t 2 + . . . a n t n + Noise (3-123) 
If the data is now fit to 

E V. 1 = 6 + a, t + a 9 t 2 + .... aj m (m < n) 
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Equation 3-123 can be written 

EV. = EV. 1 + a m+1 t m+1 + ... a n t n + Noise 


(3-124) 


If EVj consisted only of EVj the estimates of the coefficients, a Q , aj a^ 

would be correct. Errors in the coefficients are caused by the other terms in 3-124 . 

Letting 6Vj = a m+ ^t m+ ^ + ... a n t n + Noise. 


The errors in the coefficients are given by 


T 2 /2 T 3 /3 



/ 6 V- dt 

0 

T 

f t 6 Vj dt 


/ t 6 V. dt 


(3-125) 


First consider fitting the data to a quadratic function (a Q +a^t+a 2 t 2 ) with noise given by 


6V: = C [sin (Wt+0)-sin 0] 


f 6 V . dt = ^ [cos 0 - cos (WT + 0) J - CT sir 


(3-126) 


/ tV.dt = £ [-^ sin (WT + )2f - sin 0 - T cos (Wt + $) ] - (CT 2 /2) sin 0 


f tV.dt « - ~ [ T cos (WT + 0) ] - (CT 2 /2) sin 0 for WT »1 


(3-127) 


/ t 2 V.At = jjT s in (WT + 0) + 1 2 cos(WT+0) - ^ <=os0 

-T2 C os(WT+ 0)J-£T3 $jn ^ 

/ t 2 V,dt w C [ _ T 2 cos (wt + 0 ) ] - sin 0f orWT »i 
1 W 3 


(3-128) 


48 



I ' 
1 

i 


! 


For o quadratic fit equation 3-125 becomes 


6a 0 


~9T 4 

-36T 3 

30T 2 

6a| 

1 

= T 5 

-36T 3 

192T 2 

-180T 

6a 2 


30T 2 

-I80T 

180 __ 


Using 3-126, 3-127, and 3-128 

5 ai = !?£ [2 cos (Wt +0) -3 cos 0 ] 
WT 2 

The rms value over all 0 and WT is 
_ 30.5 C 

6a lrms wt 2 

Using 3-120 

30.5 C 

69 ° rms gWT 2 

5 a = 222 [ cos 0 - cos (WT + 0 ) ] 

1 WT 3 


T 

/ 

0 

T 

/ 

0 

T 

I 

0 


6 V. dt 
t6 Vj dt 
t 2 6V; dt 


The rms value over all 0 and WT is 
r. = 30C 
2,m WT 3 


Using 3-121 
rms gWT 3 

If the angle is estimated at time T, the error in the estimate is given by 


69j 


_ 6ai+26a2T _ _ 12C [ 2 cos 0 -3 cos (WT + 0 ) ] 


3WT2 


60j 


rms 


= 30 « 5C 

gWT2 


(3-129) 


(3-130) 


(3-131) 


(3-132) 


(3-133) 
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Using 3—132, 3-133, and 3-134 with = 1/2 meter and T = 100 seconds, 
the following errors can be calculated y 

^®0rms ~ 30 arc seconds j 

6 9 rms w 0.6 degrees/bour \ (3-134) 

6 0Trms « 30 arc seconds 

J 

If an accuracy of 20 arc-seconds is desired, it is obvious that a quadratic curve 
fit is not acceptable. It is also obvious that no benefit is gained by attempting to 
estimate the drift rate and using this to update the angle estimate. Therefore, it was 
decided to try a linear curve fit. 


For a linear fit, equation 3-125 becomes 


6a 0 

II 

4T 2 -6t" 

6ai 


_-6T 12 _ 


T 

/ 6 V; dt 

0 


/ tev.dt 
o ' 


(3-135) 


Using 3-126, 3-127, and 3-128 

6 a j = [ cos (WT + 0) - cos 0 ] 

WT^ 


6ai 


rms 


6C 

WT 2 


For C/W = 1/2 meter T = 100 seconds 


6^0 


rms 


= ^ =6 seconds of arc 


g 


WT 2 


This error due to sinusoidal noise is perfectly acceptable. However, from equation 
3-119 it is known that there are quadratic, cubic and higher powered second order terms 
in the data. The effects of these terms must be investigated. 

Let 6 V| = l/2g(9 d -6 0 W e i ) t 2 

Then using 3-135 

6a l = 2 ^d " 6 0 W l) T 
6 0 O = ] /2 (9 d -6 0 W) T 
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It should be noted that the error at time T in the estimate of the initial angle is 
one-half the actual change in the angle from time zero to time T due to drift rate and 
earth's rate coupling. Since the desired answer is the angle at time T and not the 
initial angle, the net result .is an error in the angle at time, T, which is equal to 1/2 the 
change in the angle due to 9 over the time interval T. This error can be significant if 
there are large drift rates or initial angle errors. 

Now consider the error due to a cubic term in the data. 

LetfiVj = 1/6 g 0 Q Wj 2 t 3 

Then using 3-135 

6 °1 = T 2 9®o W l T 2 
60 o = 0.15 0 O W 2 T 2 

Assuming 9 q = .01 

W e i = full earth's rate 
T - 100 seconds 

69g px 0.02 seconds of arc 

This error is negligible and higher powered terms in the data will have even less effect. 

From the results of this paper analysis, several conclusions can be drawn. 

First, a quadratic fit does not produce acceptable accuracy. 

Second, if a linear fit is used, there may be a problem with drift rate or earth's rate 
coupling. For example, if there is an initial angle error of .02 radians, an earth's rate 
coupling error of 0.3 seconds/second can exist. At the end of 100 seconds, the change in 
angle is 30 seconds of arc and only one half the change in angle is compensated for by the 
error in the estimate of the initial angle. This problem can be eliminated in several ways 
that are discussed as follows: 

a) Two iterations can be used, the first iteration would last for 25-30 seconds and 
reduce angle errors to about 0.5 milliradians by updating the direction cosine 
matrix in the CTMC. This procedure essentially eliminates the earth's rate 
coupling. If gyro drift is truly negligible, a second iteration of 100 seconds will 
yield an angle estimate with a 1 a accuracy of about 6 seconds of arc. 
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b) The data could be corrected for the initial misalignment by using equations 
similar to 3-1 17 and then refit. This procedure is more complex than the two 
iteration with updating procedure. 

c) The simplest, most straightforward approach is to use the technique described in 
the Crosscoupling Corrections Section of this report. This is the recommended 
approach. 

Computer. Analysis 

To confirm the results of the paper analysis with more representative noise models, 
a computer analysis was undertaken. Figure 21 shows the computer program in block diagram 
form. A polynominal in time is generated and added to noise to provide the accelerometer 
output. The noise may be generated in two ways. Either narrow band random noise with an 
autocorrelation function a y e ~ a>r cos WqT can be used or the noise can be represented as 
the sum of sinusoidal terms. The accelerometer output is quantized and fed into a least 
squares curve fit program to provide estimates of the polynominal coefficients. 

Figures 22-27 show the results of several runs made with random noise inputs. All 
runs assume a quantization level of 0.02 m/second, a noise center frequency of 1 .57 radians/ 
second and sampling time of 1 second. 

These curves show the envelope of points obtained with the individual prints marked 
except for those on the envelope. All the curves show the beating effect in the error which 
has been evident in the maximum likelihood and Kalman Filters with imperfect noise models 
and in general confirm the results of the paper analysis. 

Figure 22 shows the results of a quadratic fit after about 100 seconds time with 20 meru 
drift. Rather large errors in the estimate of the initial angle are evident. Since the change 
inangle over 100 seconds time is +30 seconds of arc, the errors in the actual angle estimate 
are even larger. 

Figure 23 shows the results of a linear fit after about 100 seconds time with 20 meru 
drift. The change in actual angle over 100 seconds time is again +30 seconds of arc. It is 
seen that the error made in the estimate of the initial angle compensates for about one half 
of this change as predicted by the paper analysis. 

Figures 24 and 25 show the errors existing around 25 seconds with a linear fit for 
two different correlation times. These results confirm that an initial iteration will reduce 
angle errors to about 100 seconds of arc. 

Figures 26 and 27 show the errors existing after 100 seconds with a linear fit and an 
initial condition of 0,0005 radians for two different correlation times. This initial condition 
would be typical for a second iteration if an iterative technique were used. However, it 
should be pointed out that the error is not a strong function of initial condition (see Table 2). 
Therefore the results are valid even if only one iteration is used. The rms angle error 
determined from these runs are shown in Table 1 . The errors are determined at each sampling 
time from 98 to 102 seconds and over the whole time interval from 98 to 102 seconds. The 
earth's rate coupling error at this time is equivalent to 0.7 seconds of arc. The error due to 
gyro drift is given by 50 0^. Oj must be small enough to make this error acceptable for this 
technique to work. 
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Fiaure 23. ERROR IN INITIAL ANGLE ESTIMATE 





I 






o 

O' 


Z 00 

O Q 


O 


O 

O 


CO o 
s: o 

C£ LU 
00 
l/) 

^ o 

£ CM 


<c 

LU 


<C LO 

♦— t 

l— o 


II 

w 



00 

Q *~n 


LU 

O' 

Z 

o 

o 

C£L 

— 1 

o 

o 

< 

CD 

az 

LU 



oc 

00 

LL. 

<C 

LU 


O 


56 


Figure 24. ERROR IN INITIAL ANGLE ESTIMATE 
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Fiaure 25. ERROR IN INITIAL ANGLE ESTIMATE 
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Table 1 

ANGULAR ERRORS AROUND 100 SECONDS 
OF TIME 


Time 

(seconds) 

RMS Angular Error 
(seconds of arc) 


T = 200 seconds 

T ~ 20 seconds 

98 

4.7 

8.0 

99 

4.5 

10.4 

100 

8.0 

7.9 

101 

6.1 

6.6 

102 

4.7 

6.8 

98-102 

5.7 

8.1 


Table 2 indicates the errors obtained using noise that is the sum of sinusoids at four 
different frequencies. The noise is approximately equal to that used in NASA's CTMC Run 
S 04400060 shown in Table 2 also. Answers were obtained for two different initial conditions and 
indicate that the error is not a strong function of the initial condition. The errors are 
slightly less than those obtained using random noise. 

Table 2 

A. ERRORS USING SINUSOIDAL NOISE 


Time 

(seconds) 

Angular Error (seconds of < 

□ rc) 

0.01 radian initial angle 

0.0005 radian initial angle 

22 

75 

83 

23 

0 

4 

24 

108 

118 

25 

81 

85 

26 

11 

16 

98 

-5.5 

-5 

99 

1 

1 

100 

8 

-7.5 

101 

-7.5 

-6 

102 

0 

0 



Table 2 (Continued) 

B. NOISE INPUT COMPARED TO CMTC RUN S4400060 


Time 

V x (m/s) 

(Seconds) 

S 04400060 

UD 

0 

0 

0 

1 

0.98 

0.95 

2 

1.21 

1.32 

3 

0.19 

0.23 

4 

0.59 

0.83 

5 

1.22 

1.10 

6 

0.72 

0.61 


Preconditioning of Data 

An analysis of the effects of preconditioning the data by integrating before curve 
fitting has been performed assuming sinusoidal noise at a single frequency. Noise is 
assumed to be 


V f = C [sin (Wt+j2f) - sin 0 ] 


The analysis follows that used in the effect of sinusoidal noise at a single frequency, 
covered earlier in this section. The integrated data is fit to a quadratic and the quadratic 
coefficient is the one of interest. Results show that 


6 Q 2 rms 


30C 

W 2 T3 


60 Orms 


_ 60C 
gW 2 T 3 


Using Sl = 1/2 meter W = 1 .5 radians/second and T 
W 

f>0Orms » 0.4 seconds of arc 


100 seconds yields 


This error is significantly smaller than that obtained when the data is not integrated 
before curve fitting. The validity of this result with more representative noise was tested 
by modifying the computer simulation shown in Figure 21 so that the generated data plus 
noise is integrated being quantized and curve fit. The integration is performed by the 
simple summing procedure shown below. 

t=K At K-l 

/ V dt = £ Vj + V« 

o i=o 
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A quadratic fit is used and the quadratic coefficient is the one of interest. 

The results using random noise with an autocorrelation function of the form 
0 (t) - Cy e - at cos u'o T are shown in Figure 13 of the results section as a function of the 
correlation time, t, of the noise. The error plotted is the rms error over the time period 
from 98 to 102 seconds from the start of the second iteration . Figure 13 also shows a 
comparison of the results obtained using non-integrated and integrated data. The non- 
integrated data is fit to a linear curve as described previously. For noise correla- 
tion times greater than about 20 seconds, the use of integrated data produces better 
results. For shorter correlation times, the use of integrated data produces worse results. 
The asymptotes on the figure are the errors when the noise is assumed to be a sine of wave 
of a single frequency (r-00). 

The results using a noise input approximately equal to that used in NASA's CTMC 
run S04400060 are tabulated in Table 3 for a 0.01 radian initial angle. 


Table 3 


Time (Seconds 

Angular Error (Seconds of arc) 


Integrated Data 

Non-integrated Data 

22 

-7 

75 

23 

+25 

0 

24 

+ 1 

108 

25 

- 5 

81 

26 

+11 

11 

98 

-0.3 

-5.5 

99 

0.2 

1 

100 

-0.2 

8 

101 

-0.4 

-7.5 

102 

+0.1 

0 


The noise in this case consists of a sum of sine waves and the results indicate a sig- 
nificant improvement in accuracy when the data is integrated. 

However, it must be recognized that the noise will at least contain a random 
component with a correlation time which is unknown at this time. Since short correlation 
times can result in a severe degradation of performance when using integrated data, the 
preconditioned data approach was not pursued further. 


Conclusions 


1) A least squares curve fit technique can be used to produce accuracies better 
than 10 seconds of arc, lcr . There are two alternate ways in which if can be 

mechanized. The first of these uses the technique described in section of this report 
to eliminate earth's rate couplings and requires a data gathering period of 100 seconds. 
This is the recommended approach. An alternate method requires two iterations with data 
gathering periods of 25 and 100 seconds respectively. This method requires that the 
CTMC be updated at the end of the first iteration to remove the earth's rate couplings. 

2) An error equal to one half the angle change due to gyro drift over a 100 second 
data gathering interval exists. Gyro drift must be small enough to make this 

error acceptable. 

3) More time is required to reach a given accuracy with this technique than with 
a Kalman filter. 


4) The equations to be mechanized are simpler than for the Kalman filter. The 
equation for a linear least squares fit is 



~£i 

St;" 

-1 

~EVj " 


St; 2 _ 


EtjVj 

L. 1 1 ^ 


Since a constant sampling interval may be used, the coefficients of the matrix 
inverse may be precalculated and stored. 

The computation problem is then reduced to solving the equation 

® = £l = brvj + c rtjVj 
g 


Here b and c are precalculated constants and the equations for calculating them are 
given in Appendix C. 


E, MAXIMUM LIKELIHOOD FILTER 


The maximum likelihood filter will be applied to determine the parameters of a 
deterministic function when measurements of this function are corrupted by noise. For many 
cases this filter yields the same results as a Kalman filter and these similarities will be dis- 
cussed later. Out total objective is to investigate errors in the filtering process when the 
assumed or modeled noise is inconsistent with the true noise, 

DYNAMIC MODEL 

We consider one axis of a platform system having an initial miserection 0 and a 
constant drift rate d. Earth's motion is not considered. Then the miserection angle 0 is 
given by 

9 = 0 + dt (3-136) 


l - 
! • 
* / 


I 


I 

{ 


An accelerometer mounted at this angle would sense gravity in the amount ‘ ■ 

a = g0 = g/9 + gdt (3-137) 


The velocity output would then be 

v * g/9t + (3-138) 


The initial misalignment and drift are random parameters, or rather random constants. 
Thus the velocity (equation 3-138 ) is deterministic and the filtering of data to determine 
the coefficients of the linear and quadratic time terms will allow the initial misalignment 
and drift to be found. 


A sequence of velocity measurements are made using an integrating accelerometer. 
The actual measurement is denoted by Z. Due principally to missile sway, the measure- 
ment is noisy. Thus, a general measurement is 


Zj « A] t. + A 2 t; 2 + Ej 


(3-139) 


where A] and A 2 are the coefficients of the deterministic terms and E is the noise input. 
For a sequence of n measurements 



(3- 140a) 
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or 


E - Z - UA 


(3-1 40b) 


We consider the error is represented by Gaussian noise with covariance matrix 
S, i.e. 


S » < E E T > 

Thus, the joint probability density function for the errors is 


(3-141) 


p(Z) = 


1 


(2ir) n/2 /S/ ] /2 


-1/2 (E T S-lE) 


-1/2 (Z-UA) T S** 1 (Z-UA) 


(2ir) n / 2 /S/ 1/2 


(3-142) 


Maximum Likelihood Estimate 

This estimate is found by determining the parameters A] and A 2 which maximize p(Z), 
equation 3-142 , for the actual set of observations. Several significant points must be 
noted-——-— 1) we assume now no apriori knowledge of the coefficients, i.e. initial 
variances of Aj and A 2 are infinite; 2) the parameter Aj and A 2 are independent; this 
would not necessarily be true if earth's rotation were considered. 

Thus, maximization of p(Z) can be done relative to A^ and A 2 directly. Let 

L(A) « (Z-UA) T S** 1 (Z-UA) (3-143) 

We maximize the taking the gradient of L(A) 

grad A L(A) * -2U T S' 1 (Z-UA) = 0 


or 

U 1 s" 1 UA = uV^Z 

So that the estimated value of the coefficients is 

A « (uV 1 U)" 1 UV 1 Z (3-144) 
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This equation provides the weighting coefficients for determining A in terms of the 
measured velocities. To estimate the error in A due to the noise, we realize that 

Z = UA + E (3-145) 

where the true but unknown value of A is used. 

Substituting for Z in equation 3-144 . 

A = (uV’u)" 1 (uV’ujA + iuV’u)' 1 UV 1 E 


or 


AA = A- A = (UW 1 U T S -1 E 


(3-146) 


We note that the estimate is unbiased since the expected value of E is zero. 
The variance of A is 


P A = <(AA)(AA) T > 


(3-147) 


Tc' 1 . 


Thus, 


In taking the transpose, we make use of the fact that S and U S U is symmetric. 


P A = <(U T S _1 U)' 1 u T s~ ] ee t s _1 U (uV'urV 


(3-148) 


We now consider that the true noise may have a covariance differing from the 
modeled noise, equation 3-141, Thus, let 

S T = < EE T > (3-U9) 

Then the variance of the estimated coefficients is 

P A = (uW 1 U T S" 1 S-rS” 1 U (uV'U)" 1 (3-150a) 

If the model is correct so that Sj = S, then 

P a = (U T S" 1 U)' 1 (3-150b) 

These are the two fundamental equations used in all later calculations to compare 
the effects of inconsistencies between modeled and true noise. 
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Example and Use of A Priori Data 


Velocity noise having an auto-correlation function 

R(n6T) = .5 n 6 t/200 cos 2 n 6 t meter^/sec 2 (3-15 

was used. Model noise matched true noise. Time spacing was 6t = 1 second and n = 25 
points were used. Results for the variance are 


pa = 


P 11 P 12 


P 21 p 22 


.281xl0 -4 -. 1 lOxlO -5 


-JlOxlO" 5 ,461xl0“ 7 


11 " 0 M 1 


A, = .281 x 10- 


Si nee 


A] = £g 
g 2 a 2 j8 = aA 1 


Og = 21 x 10^ crA^ arc-second 
or p - 111 arc-seconds 

This appears to be the error in determining initial misalignment after measure- 
ments lasting 25 seconds. 

Continuing let us evaluate the variance of the drift estimate. Here 


p 22 = ° r ^A2 = * 4 ^ x 10"^ 


Since 


A 2 = 9f 


2 

CTd ~ a a 0 = .044 x 10“^ rad/sec. = 9.05 deg/hr 
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Here we note the extremely large uncertainty in drift. However, we know apriori 
that this uncertainty is very small. This knowledge can be used to reduce all variances 
in P A . We consider an effective additional measurement. 


y = H 


where 



(3-152) 


H = [0 1] 

since only drift is measured. The variance of the noise in equation 3-152 is R and is 
very small compared to ?22‘ After ^is measurement is used P A is reduced to 

P A = P A - P a H T [HP a H T + R]- 1 HP a (3-153) 

Expanding we find 



Oo ~ 25.6 arc -seconds 

This is a much more reasonable value. An alternate approach would note that an 
uncertainty of drift of even 1 meru = . 015/arc sec/sec would cause an additional mis- 
alignment of less than an arc-second in the total measurement period. We may in effect 
continue our analysis using the maximum likelihood method to fit only a linear time 
term in order to make our error estimates. 


68 


Noise Spectrum 


A typical velocity auto-correlation function for velocity is shown in equation 
3-151 . We note however that the velocity measured is always the difference between 
an initial velocity and the one measured, i.e. 

V = V - V 

v n v n o 

Thus, the autocorrelation is 

<v m v n > = <V m , v n > - <V m , V o >-<V n , V 0 >+<V 0 , v 0 > (3-155) 

Results 

Sampling Time 

For a velocity noise auto-correlation given by equation 3-151 , a series of 
computer runs were made investigating the effect of sampling time. The noise center 
point period was 3.14 seconds. Results shown in Figure 28 indicate that for sampling times 
below 1 second, results are very nearly the same. In Figure 29 we see that large errors 
exist when the sampling period exceeds the center point period. One second sampling 
time will be used for a II the following results. 

Noise Model 

The velocity noise auto-correlation will be represented by 

R(n) = R 0 6(o) + Rj e ~ nT ^ cos Wnr (3-156) 

The study looks at the miserection error variance after 60 seconds of measurements 
fora variety of noise parameters. The filtering model here matches the true noise model. 
The model allows for a white noise component, R Q , and a sinusoidal term. In general , 

R^ will always be .5 (meters/sec) 2. 

a) Effect of Frequency 


Ro 

w 

T 

One Sigma Miserection 

0 

1.5 

200 

8,77 arc-seconds 

0 

2.0 

200 

7.03 

0 

2.5 

200 

6.19 
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Miserection Error - Arc Seconds 










Variance (j3g) 



29. EFFECT OF SAMPLING INTERVAL ON MISERECTION ERROR 






b) Effect of Correlation Time 


R o 

w 

T 

One Sigma Miserection 

0 

2 

200 

7.03 arc-seconds 

0 

2 

100 

10.30 

0 

2 

20 

22.2 

0 

2 

2 

67.5 

0 

2 

0 

114 


c) Effect of White Noise Component 


R o 

W 

T 

0 

2 

200 

.1 

2 

200 

.2 

2 

200 

All White Noise 

R o = 

.5 

R 1 


One Sigma Miserection Error 

7.03 

51.8 

72.5 


Here we see the best results which can be obtained after 60 seconds of erection 
time. For high accuracy we require that the noise be highly correlated. 

Filtering Effects 

We now consider true noise to be represented by equation 3-156 with 

R o = 0 

r\ 

Rl = .5 (meter/sec) z 
W = 2 radians/sec. 

T = 200 seconds 
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and study the effect of a filtering model which does not match the true noise. Parameter 
shown below are for the filter model 






One Sigma Miserection 


R o 

Rl 

W 

T 

Error-Arc Sec. 

Comments 

0 

.5 

2 

200 

7.03 

Perfect Model 

0 

0 

.5 

.5 

2.5 

1.5 

200 

200 

9.7 

13.0 

Mismatched Frequency 

0 

.5 

2 

100 

7.03 

Mismatched Correlation Time 

.1 

.5 

2 

200 

7.80 


.2 

.5 

2 

200 

8.18 

White noise Component 

.5 

.5 

2 

200 

8.85 


.5 

0 



17.5 

Least squares filter 


We note that it is desirable to have the filter match the noise as much as possible. 
When the characteristics of the true noise are uncertain, one can guess safely by using 
a higher frequency and shorter correlation time for the filtering model . A small amount 
of white noise should also be used to model the accelerometer quantization. 

The data below investigates the effect of two or more filter parameters which do 
not match the true noise. These lead to the same conclusions as the previous data. 


R o 

R 1 

W 

T 

One Sigma Miserection 
Error-Arc. Sec. 

Comments 

.1 

.5 

2.5 

200 

16.3 


.2 

.5 

2.5 

200 

15.3 

White noise component and 

.1 

.5 

1.5 

200 

13.5 

mismatched frequency 

.2 

.5 

1.5 

200 

13.1 


.1 

.5 

2 

20 

7.5 

White noise component and mis' 

.2 

.5 

2 

20 


matched correlation time 

0 

.5 

2.5 

20 

9.2 

White noise component, 

.1 

.5 

2.5 

20 

10.7 

mismatched frequency and 

.2 

.5 

2.5 

20 

12.8 

correlation time 
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Figure 30 compares the error variance when using a least square filter with a 
perfect model . 

Weighting Function 

Figure 31 shows the coefficients that the velocity data points are multiplied by for 
two filter models. These are presented primarily to show the affect of acceleration cir- 
cuitry in producing an early or late pulse. The error caused is the difference between 
weighting coefficients for two adjacent points. This is about .6 x 10"^. For a velocity 
quantization of .02 meter/sec., this causes an error of 

/} = — - — = .122 x 10"^ radians = .025 seconds which is negligible . 

9*8 

Relation to Kalman Filter 


When the dynamic model, equation 3-136 and 3-137 is proper, the Kalman filter 
will give identical results as the maximum likelihood filter. 

If apriori data is available as discussed in the example here, proper use of this 
data conserves similarity between these two filtering methods. The Kalman filter does 
allow a more realistic dynamic model in which earth's rotation is considered without 
adding complexity to the computations and more easily allows additional data such as 
the azimuth measurement to be entered into the filtering process. 


r 

i 
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Weighting Coefficient 




F, CORRECTION FOR BIAS ERRORS IN 
THE LEAST SQUARES FILTER 


It was pointed out in Section lll-C that a bias exists in the angle estimates for both 
vertical and azimuth. The azimuth bias is due to vertical error bias which in turn is due 
to earth's rate crosscouplings. An expression for an optimal correction matrix was derived 
and it was pointed out that the simpler matrix derived for a least squares filter would pro- 
vide adequate accuracy. The purpose of this section is to derive the simple correction 
matrix. 

This matrix is based on the assumption that the difference between the actual and 
measured angles can be expressed in terms of the actual angles, the geometry of the 
theodolite measurement, and earth's rate. The coordinates systems used a r e shown in 
Figure 18. 


The azimuth measurement is given by 
mj = a + tan y Q [0 sin $ + y cos f ] 

Where 


0 ! 

P 

y 

y 

'o 


s the actual azimuth angle 
is the actual miserection angle about the j axis 
s the actual miserection angle about the Z axis 
s the theodolite elevation angle 

s the angle between the theodolite line of sight and the Z Q axis 


Using the results of Appendix C (Equations C-4 and C-5) the vertical measurement 
can be written as 


m 2 = 1/2 aT + 0 - 1/2 co x y T 

m 3 -- 1/2 U> <yT + 1/2 &u x 0T + y 

Where T equals the time of the measurement u> x , U> y , tO z are the components of earth's 
rate about the x c , y Q , and z Q axes. 


3-158. 

3-159 
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1 

I 

■HL, 


S' 

£ 


r ; 


sy-'* 

r. 


n - 
a 


Eauations 3-157, 3-158 and 3-159 can be solved simultaneously to yield 


a 


a ll a 12 a 13 




— 

a 21 a 22 a 23 


m 2 

y 


_ a 31 a 32 a 33_ 


J"3 


The matrix coefficients are given by 
a n = (1 + 1/4 a> x 2 T 2 )/A 

a 12 = 0/2 C2^ x T - C])/A 

ai 3 = (-C 2 -1/2Ci» x T)/A 

a 2 i = 0/4 a> x U> y T 2 - 1/2 a) Z T)/A 

022 = (1 + 1/2 , C2U)yT)/A 

a 23 = (l/2a> x T+ l/2C 2 to z T)/A 

a 3 i = (l/4cc x a> z T 2 +l/2co y T)/A 

a 32 = (“1/2 Cj tOyT - 1/2 co x T)/A 

a 33 “ (1 ~ 1/2 C, a> z T)/A 

A = 1 + 1/2T [ C 2 w - C, w z ] + 1/4 T 2 a> x [C 1 <u y + C 2 a) Z +60 x ] 

C| = tan y 0 sin f 
C 2 = tan y Q cos t 


Equation 3-160 is completely valid only if there are no errors in the vertical angle 
measurements except for earth's rate coupling effects. There will be errors due to imperfect 
filtering of sway velocity. 


However, examination of the magnitude of the coefficients of the matrix indicates 
that these errors should have only a small effect on the correction applied. To confirm 
this conclusion, two runs were made with the Erection and Alignment Simulation Program. 


U 


5 1 
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In each of these runs, an initial condition of 2 degrees on each of the three axes was 
assumed. In one case, a zero sway velocity noise was used, and in the other case a sway 
velocity noise of 0.5 m/sec rms at a center frequency of 2 radians/second with a correla- 
tion time of 200 seconds was used. The results are tabulated in Tables 1 and 2. 


Table 1 

Angular Errors Before and After Correction for 
Earth's Rate Coupling (Zero Noise) 


Angular Error (secs of arc) 

Kalman Filter Estimates 

Least Squares Estimates 

a P y 

a p y 


Original Estimate 

-7.6 

-8.3 

— 

-7.1 

-7.9 

Estimate after Correction 0.3 

-.7 

-1.1 

.1 

-.2 

-.7 

Amount of Correction Applied 

6.9 

7.2 

— 

6.9 

7.2 



Table 2 

Angular Errors Before and After Correction for 
Earth's Rate Coupling (0.5 m/sec noise) 




Angular Error (secs of arc) 



Kalman Filter Estimates Least Squares 

Estimates 


a P 

y a p 

y 

Original Estimate 

10.7 

-.5 -- 14.5 

6.8 

Estimate after Correction -8.6 17,6 

6.7 -10.5 21.4 

13.9 

Amount of Correction 

Applied — 6.9 

7.2 — 6.9 

7.1 


It can be seen that with zero noise the correction scheme removes the error due to 
earth's rate coupling to an accuracy of about 1 second of arc. Secondly, it can be seen 
that the correction applied to remove the earth's rate coupling varies only by about 0.1 
seconds of arc when the noise has a value of 0.5 m/sec. Thus, it can be concluded that 
the correction matrix will accurately remove the errors due to earth's rate coupling even 
in the presence of noise. Unfortunately, in the run shown, the vertical errors both in- 
creased when the correction was made. In this case, the errors due to sway velocity noise 
and the errors due to earth's rate coupling tended to cancel . They will not tend to cancel 
each other in all cases, however, and in a statistical sense the answer has to be improved 
by removing the bias due to earth's rate coupling. The concept of the use of the correc- 
tion matrix was developed too late in this study to allow a rigorous statistical analysis of 
the improvement in performance. 


IV, DESCRIPTION OF THE ERECTION AND 
ALIGNMENT SIMULATION PROGRAM 


A, THE PROGRAM EQUATIONS 

The erection and alignment program solves the Kalman filter equations given by 
equations 3-94 through 3-100 and the least squares filter equations given by equationC-3 
for each channel. In addition, the program simulates a theodolite measurement at the 
final time. This is done by extrapolating the initial angles to time tf by the equation. 

e(tf> = (t f , t 0 ) e (t 0 ) 

Where 0]] (tf, t Q ) is approximated by: 

0n ^f' f °^ = 0 " Ofrrg] 

The theodolite measurement is then given by: 

m 1 = a(tf) + C] 0 (tf) + C 2 y(tf) 

Where C] = tan y Q sin f 
C 2 = tan y Q cos f 

The program calculates the correction matrix derived in Section lll-F and then 
calculates the final estimates of a, and y . 

B. DEFINITION OF THE PROGRAM CONSTANTS 

There are 16 floating point constants which must be read in. They are called C(l) 
through C ( 1 6) . They are defined by 

Program Constant Definition 


C(1) 


rms angle error (arc seconds) 

C(2) 

«r v - 

rms sway velocity (m/sec) 

C(3) 

f 0 - 

sway velocity center frequency (cps) 

C(4) 

T 

sway velocity correlation time (seconds) 

C(5) 

g 

gravity (m/sec)2 

C(6) 

A - 

acceleration quantization level (m/sec) 
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Program Constant 


Definition 


c (7) 

At 

- time between measurements (seconds) 

C(8) 

frf 

- final time (seconds) 

C(9) 

a> 

- earth's rotation rate (rad/sec) 

C(10) 

L 

- latitude of launch site (degrees) 

can 

B 

- target bearing (west of north) (degrees) 

C(12) 

7o 

- theodolite elevation angle (degrees) 

C(13) 

f 

- angle between axis and theodolite LOS (degrees) 

- initial misalignment angles in degrees 

C(14) 
C( 1 5) 

afro) 

Hfro) 

C(16) 




a, j8/ and y are defined by Figure 18 . When B is equal to zero, the Z axis is north. 

C, INPUT CARD FORMAT 

The format of the input cards is listed in Table 4. 

Table 4 

INPUT CARD FORMAT 


Card No. 

Variable 

Format 

Description 

1 

NNR UN 

12 


NUMBER OF RUNS 

2 

IPRNT 

12 


= 1 - PRINT FILTER GAINS AND 





CORRECTION MATRIX, = 0 - 
BYPASS 

3 

CO) 

E20.8 

C'S DESCRIBED ABOVE 

4 

C(2) 




5 

C(3) 




6 

C(4) 




7 

C(5) 




8 

C(6) 




9 

C(7) 




10 

C(8) 




11 

C(9) 




12 

C(10) 




13 

C(ll) 




14 

C(12) 




15 

C(13) 




16 

C(14) 




17 

C(15) 

) 

t 


18 

C(16) 

E20.8 
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Table I (Continued) 


Card No. Variable Format 


Description 


19 

20 
21 


I 


NUMBER, DA,TE 
T(IS), XD(1), 
XD(2) 


80H 

12, A6, A2 
3X, E14.7, 22X, 
E14.7, 4X, E14.7 


HOLLERITH IDENTIFICATION 
CASE NUMBER, DATE 
TEST DATA - Time, y velocity, 
^ Z velocity 


MULTIPLE CASES BEGIN WITH CARD 19, IDENTIFICATION 


D. PROGRAM OUTPUT 

The program output consists of a printout. First the program constants defined in 
Part B of this section are printed out in three rows. The first row contains C(l) through 
C(6). The second row contains C (7) through C(12) and the third row contains C(13) 
through C(16). Then the estimates of j8 and y given by the Kalman and least squares filter 
equations are printed out at each sampling time. Then the final angle estimates at the end 
of the filtering period are printed out. These estimates are. calculated using the correction 
matrix of Section lll-F. The actual angles at the end of the filtering period are then 
printed out. If the printout of the filter gains option is chosen, the 4 gains used in the 
Kalman filter at each sampling time and the elements of the correction matrix used to de- 
termine the final angle estimates at the end of the filtering period are printed out. 
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V, DEFINITION OF COMPUTER REQUIREMENTS 


A, ASSUMPTIONS 


1) 

2 ) 

3) 

4) 

5) 

6 ) 

7) 

8) 


These requirements for storage and execution time are based on the character- 
istics of the RCA 1 10-A computer. The execution time estimates assume fixed 
point calculations. 

It is assumed that certain calculations can be made each time a data point is 
received. This is not a necessary assumption but it reduces the required storage 
since each data point does not have to be stored. 

In determining the azimuth requirements, the multiplication 2 q is assumed 

to be performed using a conventional matrix multiplication algorithm with 
several loops. Since the other matrices contain many ones and zeros, the multi- 
plications involving them do not use the loop type algorithm in order to save 
execution time. A slight penalty of about 50 storage words is incurred by this 
approach. 

When defining both the storage required and the execution time, it is assumed 
that an increase of 50% in the arithmetic instructions will be required for 
scaling. 

No allocation has been included for program self-checking provisions. 

The filter weights are assumed to be precalculated and stored. 

The program used for determing the requirements is configured so that all read- 
in constants are saved and initial conditions of variables and index registers 
are set without further read-in if more than one pass is required due to a hold 
on the launch sequence. 

The CTMC velocity registers are assumed to be set to zero before start. 


B. EQUATIONS 

1) Azimuth - The equations used are based on those shown in AIAA paper No. 
67-556 "Initial Alignment of a Strapdown Inertial Reference and Navigation 


System" by Hans F. Kennel. 




°11 

a 12 

a 13~ 

C6“ ] C 2 2 C 4 4 C 5 5 C6~ 

a 21 

a 22 

a 23 


°3\ 

a 32 

°33_ 
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The element’s of 1^ are obtained from the CTMC. The elements of and 5q^ are 
fixed constants which are read in. The elements of 4^^ ere calculated from the porro prism 
encoder angle and a fixed misalignment. 


After calculating the elements of 1^ the following calculations are required, 

an sin yO + a 12 


sm 

cos y^ 

L yl 

L Z1 


\/a 2 12 - sin 2 y Q + a 2 u 


9 9 

a ll + a 12 


\/l - sin 


y r 


21 sIn z P + °22 cos y p 

3i sin r P + a 32 cos y p 


.-1 ^yl 
L Z1 


m l - 2 + fan 

m^ is the azimuth measurement to be used in the correction equations, 


2) Kalman Filter Equations for Level - The equations to be solved are equations 
3-94 to 3-97 in Section III. The K* (t;) are precalculated stored constants and 
are calculated by the Erection and Alignment Simulation program for each data 
(joint. The equations are of a recursive form and the four components of each 

x are calculated each time data is received. 

3) Least Squares Equations for Level - The equation to be solved for each channel 
is given by equation C-3. Each time a data point is received the S V. and 
DtV| are calculated. At the end of the filtering period the two summations 
are multiplied by stored constants and the resulting products added to provide 
an angle estimate. 

4) Corrections Equations for Bias Errors - These equations are given by equation 
3-160 of Section lll-F, The a's are precalculated and stored constants, ml 
is obtained from the azimuth calculation, m^ and m 2 are obtained from the 
level equations. 


C, COMPUTER REQUIREMENTS 

The storage requirements are tabulated in Table 5. 
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Table 5 

STORAGE LOCATIONS REQUIRED 



Program 

Stored 

Scratch 

Scaling 



Storage 

Constants 

Pad 

Instructions 

Total 

Azimuth 

171 

27 

43 

20 

261 

Kalman Filter 

77 

247* 

16 

13 

353 

Least Squares Filter 

34 

5 

8 

4 

51 

Correction Equations 

19 

9 

8 

2 

38 


* Kalman filter assumed 60 data points. For each additional data point, four additional 
storage locations are required to store weights. 


Total storage requirements, assuming scratch pad is shared, are 636 locations if the 
Kalman Filter is used and 342 locations if the Least Squares Filter is used. 

The execution times are listed below. 

Azimuth 


0.085 seconds plus 1 pass through the sin/cosine subroutine, 2 passes through the 
square root subroutine and 1 pass through the arc tangent subroutine is required. The time 
to execute these subroutines in RCA-1 10A computer was not listed on the books provided. 

Kalman Filter 

When each data point is received, 0.02 seconds plus 1 pass through the sin/cosine 
subroutine is required. In addition 0.00086 seconds is required to zero initial conditions 
at start of the filtering process. 

Least Squares Filter 


When each data point is received 0.0048 seconds is required. In addition, 0.003 
seconds is required to zero initial conditions at the start of the filtering process and to 
perform calculations at the end of the filtering process. 

Correction Equations 

0.0095 seconds is required. 
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APPENDIX A - 

FILTERING PROBLEM WHEN ERECTING WITH 
STRAPED DOWN ACCELEROMETERS 



PURPOSE 

The purpose of this appendix is first to derive the equations describing the output 
of strapped down accelerometers in a rotating and translating vehicle. The magnitude 
of the various terms in these equations are then discussed and the terms which must be 
filtered out are determined. 

DERIVATION OF THE ACCELEROMETER OUTPUTS 

■— r— •— 

The coordinate systems to be used are shown in Figure A-l . The i Q , j £ k 
system is an inertial system with origin at the center of the earth. The i, j,^ system 
is earth fixed with its origin at the center of the earth. The j', k 1 is missile fixed 
with its origin at the center of rotation of the missile. The position vector of the 
accelerometers is given by R Q in the inertial system and is made up of the components R, 
the vector from the center of the earth to the center of rotation of the missile and 7, the 
vector from the missile center of rotation to the accelerometers. 

Then R Q = ^ + r 

= R x i + Ryj + R z k + r x i' + r y j' + i^k' (A-l) 


The acceleration of the accelerometers is given by the second derivative of R Q with 
respect to inertial space. 


R 0 - R x i + Ryj + R z K + R x i + R y j + R z k + r x i' + r y j' 
r z k' + r x i' + r v j' + r'k' 


(A-2) 


The first group of three terms in A-2 is the velocity of the origin of the 
primed system with respect to the earth and will be called V. The second group of 
three terms in A-2 is the velocity due to the rotation of the earth and can be written 
Wg X R. The third group of three terms in A-2 is the velocity with respect to the 
missile-fixed system and, thus, is zero. The fourth group of three terms in the 
velocity with respect to the origin of the primed system due to rotation of the primed 
system and can be written (Wg + W m ) X r. Here Wg is the rotation of the earth and 
W m is the rate of rotation of the missile with respect to the earth. 


A-l 


Hence 


Then 


bui 


R q = V + W E X (R +") + w m X r 


R q = V + W E X (fc +"r) + X7 + W m x~r 


V = R x T + R y j + R z K + R X I + Ry] + R z K = A + (W E x V) 

Where A is the acceleration of the origin of the primed system with respect to the 
earth, then A-4 can be written 

t Q = A + (W E x V) + W E x[v + (W E x R)] + W E x [(WE + W m ) x7] 
+ x7 + W m X [(W E + W m ) x7] = A + 2 (W E X V) + W E X 
[W E x (R + 7[j + W m x (W m x r) + W E x (W m x r) + W m x 
(W E x‘r) + W m x7 
If W m is written as 


Then 


w = w 7* + w 7' + w K' 

vv m vv mx vv myJ v 'mz 

i - z 

W m " V + W myi' + W mz K’ * W mx i' + W my j' + W mz K' 

= W m + (« m + « E ) X 
= W m + » E » W m 

T 

Where W m is the angular acceleration of the primed system with respect to the 
earth. Using A-6, the last three terms in equation A-5 become 

X = W E X (W m x”r) + w m x (W E x7) + W m x7 = W m x7 + (W E x W m ) x 7 
+ W E x (W m x r) + w m x (W E x r) 


(A-3) 

(A-4) 


(A-5) 

(A-6) 
(A -7) 


A-2 


Then using the relationships 


(P X Q) X R * (R • P) Q - (R • Q) P 
R X (P X Q) « (R • Q) P - (R • P) Q 

X = W rn x7 + (W E • 7) W m - (7 • W m ) W E + (We 4 r) W m - (W E • WJ 7 
+ (w m . 7)w E -(W m • w E )7 = w m x7 + 2 
X [(W E * 7) W m - (W E • w m ) r 3 

X = w m x7 + 2 W E X (W m x7) (A-8) 

Using A-7 and A-8 in A-5 and the fact that the accelerometers read g + R Q yields 
Accelerometer B g + ^ w . x (w £ x (R x 7)]j+ A + W m x7 + W m x (W m x 7) 

+ 2 W E x [(W m x7) + V] (A-9) 

DISCUSSION OF THE ACCELEROMETER OUTPUT 


Of interest here are the horizontal components of equation A-9 since these are 
the outputs of the transformation computer that will be used to erect the computing 
coordinate system in the computer. When the system is erected, these outputs should 
be zero. Therefore, it is any non-zero horizontal components of equation A-9 which 
must be fi Itered out . 

The first two terms of equation A-5 are due to gravitational attraction and the 
centrifugal acceleration of the earth and are essentially directed along the local 
vertical (R >> r). The second three terms represent the acceleration relative to the 
earth and are caused by missile sway in the case of interest here. The last term in 
equation A-9 is the Coriolis acceleration and is proportional to the velocity relative 
to the earth . 

For missile sway then, both the acceleration and velocity relative to the earth 
will be essentially horizontal and have magnitudes related by the equation A " WV 

Where? A = magnitude of the acceleration 
V = magnitude of the velocity 
W = frequency of sway motion 


Then the ratio of the horizontal acceleration to the Coriolis acceleration is equal to 
or greater than W . For a W of 1 -radian -per-second, this ratio is on the order of I0^« 

TW7 

Therefore, the Coriolis acceleration can be neglected and the quantity to be filtered is the 
output of the accelerometer due to sway acceleration. Writing the sway acceleration as 

a = a sin (Wt + 0 ) (A-10) 

The integrated output of the accelerometer; i.e. the velocity indicated by the 
accelerometer is V - cos (Wt + 0 ) +J-L cos 0 (A-l 1) 

w w 

It should be noted that even though the input acceleration is oscillatory, the 
output of the accelerometer has a constant term in its integral. The magnitude of this 
term is a function of the phase angle of the sway acceleration at the time the inte- 
gration procedure is started. The filtering technique must then be concerned not only 
with the elimination of oscillatory error terms but also with the elimination of con- 
stant error terms . 


A-4 





APPENDIX B 

OPTICAL ALIGNMENT EQUATIONS 


The purpose of this appendix is to explain the azimuth angle equations contained in 
the report "Initial Alignment of a Strapped-down Inertial Reference and Navigation System" 
by Hans F. Kennel. Specifically, equation 16 and the equation at the top of page seven 
for 6 A, the azimuth error caused by leveling error will be derived. This work was done to 
assure that we had a proper understanding of the coordinate systems involved and the 
relationship between azimuth and leveling errors. 

Figure B-l shows the relationships between the various coordinate systems used in 
Kennel's report under the conditions that the system has been leveled (that is: coordinate 
systems 1 and 3 are coincident). The azimuth angle measurement desired is / the angle 
between the theodolite reference and the axis. The components of the line of sight in 
the 6 system are given by 


^x6 

L L y6 - 


- sin 
cos y 

0 P 


Then the components in the 1 system are given by 


a 12 a ] 3 

a 22 a 23 

a 32 a 33 


cos y, 


where the a matrix is the transformation matrix between the 1 and 6 systems. When the 
1 system is level, this matrix is a function of missile tilt, the porro-prism encoder angle, 
and the known misalignment 6 's. 

But Ly], and L z i can also be written in terms of the angles cr ^ and 7 q, the 
elevation or the line of sight in the leveled coordinate system 

L y 1 = cos y 0 cos a j 

L z ] = -cos 7 q sin a . 


L y1 

j— — ctn o\ 


B-l 



but 


, / /o\ — tan 0,1 “ tan 

Mtv - 7T/2) - ___ 7 ^— 

1 1 + tan tt/ 2 tan (yj 


= -ctn a] 


o^ = ir/2 + tan 


-1 


>1 

"Lzl 


which is equation 16 of Kennel's report. 


(B— 2) 


The remaining thing to be determined is the error in the measurement when system 
1 is not level. In Figure B-2 the non-level system 1 is denoted by primes and is mis- 
leveled by small angles a about z^ and j8 about Y j . 


For small angles, the coordinate transformation matrix from the 1 system to the 1' 
system is: 


but 




1? 

L xl 


L yl 


L zl 

— 



y 

1 

0 

1 

0 

0 


so 


"0 

0 

1 


0 0 
cos ai +sin al 
-sin a] cos of 1 


-sin 7q 
cos 7 0 
0 


(B-3) 


L xr 


1 

y 

-0 

Lyl' 


-y 

1 

0 

Lzl' 


0 

0 

1 


comparison of B-3 and B-4 indicates that 


1 

0 


0 0 


-sin yQ 

cos of 1 +sin a] 


cos 7() 

-sin Of] cos of] 


0 

_j 

— ^ 


(B-4) 


Lyl* = Lyl + y sin Yo 

L Z 1' = L zl "0 sin 70 


Then the errors in the y and z components of the line of sight when system 1 is not 
level are given by 


AL y1 = y sin y 0 
AL zl = -0 sin y Q 


(B-5) 


B-3 



The error in o'] is given by 

6 A = d -?1. AL y 1 + A Li 

SL y l y d L„i 


Using B-2 

6a = 


ny 


{[ L 'i 


Zl_ AL, 


(B-4) 


From B-3 

Ly] = cos or 1 cos 7() 
t- z j = -sin a] cos 7 q 


(B-7) 


Substituting B-5 and B-7 into B-6 yields 

1 J ysin yo 

7 O'! S sin 0! 


6A 


1 + 


COS O'! 

— — l 

sin O ' 1 v 


1 cos 7 q 


0sin 7 qcos a; 1 cos 7o 
sin 3 a] cos 3 y q 


6 A - tan 7o 7 sin a] + 0 cos 

But - 7sin 0!] + j8 cos a] is the tilt about the horizontal projection of the 
ine of sight and equals 6 l* 


(B-8) 


6a = 6|_ tan 7 q which is the equation in Kennel's report. 


An estimate of calculated using equation B-2 can be used in equation B-9 
to determine a correction to be applied to the original estimate of a] to obtain a 
better estimate. The correction can be calculated using estimates for 7 and (i obtained 
from the erection system. 


APPENDIX C 

CLOSED FORM SOLUTIONS TO THE LEAST 
LEAST SQUARES EQUATION 


The form of the linear least squares curve fit equation is 


pA - 
a 0 


K 

K 

-1 

K 


2 i 

i=l 

2 tj 


2 V. 

i=i ' 



— 

i- l 




K 

K 


K 

jh_ 


s n 

i=l 

2 t 3 ; 

i=1 


2 t, V, 

i=l 



L. 

— J 


— — j 


The estimate of the erection angle, 0 is given by 



g ' 

When the data is taken at constant time intervals 6t 


K ^ 

l = K 

i=l 


y , _ 6t (K) (K+l) 

1=1 

K 3 

E t 2 , = 2L (K) (K+l) (2K+1) 
i=l 6 

Using C-2, and C-l yields 

® = ~ = " g6f(K)(K-l) .? V ‘ 

g i=i 




12 

+ gfit 3 (K) (K-l) (K+1) 


K 

I IV; 


i=l 


(C-2) 


(C-3) 


C-l 


Equation C-3 can also be used to derive an exact equation for the angular error 
resulting from a quadratic term in the data. The data is assumed to contain only a 
quadratic term of the form 1/2 g0t s . Using C-l, C-2 and the fact that 

? t* - 

1=1 4 

yields 

& = = (1/2)0 Kfit ^ (C-4) 

Equation C-4 is the exact equation for the error. In deriving the equations to 
be used to correct for earth's rate coupling in section lll-F, it was assumed that the 
error was © e - 1/2 0 T where T = K6t (C-5) 

Comparison of equations C-4 and C-5 shows that C-5 is correct to about 1% when 
100 data points are taken at 1 second intervals. This error is perfectly acceptable. 


C-2 



